// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PatchCluster.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Vision.ObjectDetection
{
    using System;
    using System.Collections.Generic;
    using System.Windows;

    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision;

    /// <summary>
    /// Represents a cluster of image patches
    /// </summary>
    public class PatchCluster : IComparable<PatchCluster>, IShape2D
    {
        /// <summary>
        /// Provisional center X
        /// </summary>
        private double centerXTemp;

        /// <summary>
        /// Provisional center Y
        /// </summary>
        private double centerYTemp;

        /// <summary>
        /// Initializes a new instance of the <see cref="PatchCluster"/> class with a single
        /// cluster element
        /// </summary>
        /// <param name="x">Element x position</param>
        /// <param name="y">Element y position</param>
        /// <param name="z">Element z position</param>
        /// <param name="confidence">Element confidence</param>
        /// <param name="left">Left edge of element</param>
        /// <param name="right">Right  edge of element</param>
        /// <param name="top">Top  edge of element</param>
        /// <param name="bottom">Bottom  edge of element</param>
        internal PatchCluster(double x, double y, double z, double confidence, double left, double right, double top, double bottom)
        {
            this.Components = new List<CandidateCenter>();
            this.Left = left;
            this.Right = right;
            this.Top = top;
            this.Bottom = bottom;
            this.PatchHeight = bottom - top;
            this.PatchWidth = right - left;
            this.centerXTemp = x;
            this.centerYTemp = y;
            this.Components.Add(new CandidateCenter(x, y, z, confidence));
        }

        /// <summary>
        /// Gets left cluster edge
        /// </summary>
        public double Left { get; private set; }

        /// <summary>
        /// Gets right cluster edge
        /// </summary>
        public double Right { get; private set; }

        /// <summary>
        /// Gets top cluster edge
        /// </summary>
        public double Top { get; private set; }

        /// <summary>
        /// Gets bottom cluster edge
        /// </summary>
        public double Bottom { get; private set; }

        /// <summary>
        /// Gets the patch width estimate
        /// </summary>
        public double PatchWidth { get; private set; }

        /// <summary>
        /// Gets the patch height estimate
        /// </summary>
        public double PatchHeight { get; private set; }

        /// <summary>
        /// Gets ordered distances of all cluster points to estimated pose position
        /// </summary>
        public List<KeyValuePair<int, double>> Distances { get; private set; }

        /// <summary>
        /// Gets component list in cluster
        /// </summary>
        public List<CandidateCenter> Components { get; private set; }
        
        /// <summary>
        /// Gets maximum distance of cluster elements to estimated pose position
        /// </summary>
        public double MaxDistance
        {
            get
            {
                if (this.Distances != null)
                {
                    return this.Distances[this.Distances.Count - 1].Value;
                }

                return 0.0;
            }
        }

        /// <summary>
        /// Gets the width of cluster in pixels
        /// </summary>
        public int WidthInPix
        {
            get
            {
                return (int)(this.Right - this.Left + 0.5);
            }
        }

        /// <summary>
        /// Gets the width of cluster in pixels
        /// </summary>
        public int HeightInPix
        {
            get
            {
                return (int)(this.Bottom - this.Top + 0.5);
            }
        }

        /// <summary>
        /// Gets standard deviation of cluster points to estimated pose position
        /// </summary>
        public double DistanceStdDev { get; private set; }

        /// <summary>
        /// Gets the mean deviation of cluster points from estimated pose position
        /// </summary>
        public double DistanceMean { get; private set; }

        /// <summary>
        /// Gets or sets overall estimate of pose
        /// </summary>
        public ObjectPoseEstimate PoseEstimate { get; set; }

        /// <summary>
        /// Gets the estimated mean confidence
        /// </summary>
        public double MeanConfidence { get; private set; }

        /// <summary>
        /// Gets the standard deviation of the confidence estimate
        /// </summary>
        public double StdDevConfidence { get; private set; }

        /// <summary>
        /// Gets the confidence of the cluster
        /// </summary>
        public double Confidence
        {
            get
            {
                if (null != this.PoseEstimate)
                {
                    return this.PoseEstimate.Confidence;
                }

                return 0.0;
            }
        }

        /// <summary>
        /// Gets a one dimensional value indicative of the patch size. Defined as the 
        /// half the maximum of patch width or height
        /// </summary>
        public double Size1D
        {
            get
            {
                return Math.Max(this.PatchWidth, this.PatchHeight) / 2.0;
            }
        }

        /// <summary>
        /// Gets the center X of the cluster
        /// </summary>
        public double CenterX
        {
            get
            {
                if (this.PoseEstimate != null)
                {
                    return this.PoseEstimate.CameraPixelPose.Position.X;
                }

                return 0.0;
            }
        }

        /// <summary>
        /// Gets the center Y of the cluster
        /// </summary>
        public double CenterY
        {
            get
            {
                if (this.PoseEstimate != null)
                {
                    return this.PoseEstimate.CameraPixelPose.Position.Y;
                }

                return 0.0;
            }
        }

        /// <summary>
        /// Attempt to add a candidate patch to the cluster. Will add if patch lies in cluster otherwise is rejected
        /// </summary>
        /// <param name="x">Element x position</param>
        /// <param name="y">Element y position</param>
        /// <param name="z">Element z position</param>
        /// <param name="confidence">Element confidence</param>
        /// <param name="left">Left edge of element</param>
        /// <param name="right">Right  edge of element</param>
        /// <param name="top">Top  edge of element</param>
        /// <param name="bottom">Bottom  edge of element</param>
        /// <returns>True if added, false if not added</returns>
        public bool Add(double x, double y, double z, double confidence, double left, double right, double top, double bottom)
        {
            bool isAdded = false;

            if (x >= this.Left && x <= this.Right && y >= this.Top && y <= this.Bottom)
            {
                double maxDx = Math.Abs(x - this.centerXTemp);
                double maxDy = Math.Abs(y - this.centerYTemp);
                double widthBy2 = this.PatchWidth / 2.0;
                double heightBy2 = this.PatchHeight / 2.0;

                if (maxDx <= widthBy2 && maxDy <= heightBy2)
                {
                    int count1 = this.Components.Count;
                    this.Components.Add(new CandidateCenter(x, y, z, confidence));
                    this.Left = Math.Min(this.Left, left);
                    this.Right = Math.Max(this.Right, right);
                    this.Top = Math.Min(this.Top, top);
                    this.Bottom = Math.Max(this.Bottom, bottom);
                    this.centerXTemp = this.centerXTemp * (count1) / this.Components.Count + x / this.Components.Count;
                    this.centerYTemp = this.centerYTemp * (count1) / this.Components.Count + y / this.Components.Count;
                    this.PatchWidth = (this.PatchWidth * count1 + (right - left)) / this.Components.Count;
                    this.PatchHeight = (this.PatchHeight * count1 + (bottom - top)) / this.Components.Count;
                    isAdded = true;
                }
            }

            return isAdded;
        }

        /// <summary>
        /// Estimate the position and confidence of the cluster center
        /// Position is simply the expected position (weighted sum of position)
        /// For overall confidence do a pseudo bayesian estimate where the prior is zero
        /// Elements with no reading kinect values are ignored for depth estimation
        /// This is a very simple implementation for now and needs more work (task 1901)
        /// </summary>
        /// <param name="noReading">Kinect no reading value (z)</param>
        public void Estimate(short noReading)
        {
            if (null == this.Components || this.Components.Count <= 0)
            {
                this.PoseEstimate = null;
                return;
            }

            const double Prior = 10.0;
            double sum = 0.0;
            double sumZ = 0.0;
            double expectedConf = 0;
            double varConf = 0;
            double err = 1.0 - sum;
            Vector3 pos = new Vector3();
            this.MeanConfidence = 0.0;
            this.StdDevConfidence = 0.0;

            foreach (CandidateCenter c in this.Components)
            {
                sum += c.Confidence;                
                if (c.Center.Z < (double)noReading)
                {
                    pos = pos + (c.Center * c.Confidence);
                    sumZ += c.Confidence;
                }
                else
                {
                    pos = pos + c.Confidence * (new Vector3(c.Center.X, c.Center.Y, 0));
                }

                double conf2 = c.Confidence * c.Confidence;
                expectedConf += conf2;
                varConf += conf2 * c.Confidence;
            }

            if (sum > MathConstants.ErrorEpsilon)
            {
                double z = pos.Z;
                if (sumZ > MathConstants.ErrorEpsilon)
                {
                    z = pos.Z / sumZ;
                }

                pos = pos / sum;
                expectedConf /= sum;
                this.StdDevConfidence = (varConf / sum) - (expectedConf * expectedConf);
                this.StdDevConfidence = (this.StdDevConfidence > 0.0) ? Math.Sqrt(this.StdDevConfidence) : 0.0;
                this.MeanConfidence = expectedConf;
                pos.Z = z;
            }

            this.Distances = new List<KeyValuePair<int, double>>();
            double minDistance = (pos - this.Components[0].Center).Length();
            minDistance = minDistance * minDistance;
            int minIdx = 0;
            this.DistanceStdDev = 0.0;
            this.DistanceMean = 0.0;

            for (int i = 0; i < this.Components.Count; ++i)
            {
                double d = (pos - this.Components[i].Center).Length();
                d = Math.Abs(d);
                this.DistanceMean += d;

                if (d < minDistance)
                {
                    minIdx = i;
                    minDistance = d;
                }

                this.DistanceStdDev += (d * d);
                this.Distances.Add(new KeyValuePair<int, double>(i, d));
            }

            this.DistanceMean /= this.Components.Count;
            this.DistanceStdDev = this.DistanceStdDev / this.Components.Count - this.DistanceMean * this.DistanceMean;
            this.DistanceStdDev = (this.DistanceStdDev > 0.0) ? Math.Sqrt(this.DistanceStdDev) : 0.0;
            this.Distances.Sort((first, second) => 
            {
                return first.Value.CompareTo(second.Value);
            });

            if (this.MaxDistance >= 0.0)
            {
                expectedConf = (this.Components[this.Distances[0].Key].Confidence * this.Components.Count) / (this.Components.Count + Prior);
                this.PoseEstimate = new ObjectPoseEstimate(new Pose(pos, new Quaternion(0, 0, 0, 1)), expectedConf, 0.0);
            }
        }

        /// <summary>
        /// IComparable interface implementation
        /// </summary>
        /// <param name="other">Other element to compare</param>
        /// <returns>Standard comparison value</returns>
        public int CompareTo(PatchCluster other)
        {
            double diff = this.PoseEstimate.Confidence - other.PoseEstimate.Confidence;

            if (diff > MathConstants.ErrorEpsilon)
            {
                return -1;
            }
            else if (diff < MathConstants.ErrorEpsilon)
            {
                return 1;
            }

            return 0;
        }

        /// <summary>
        /// Candidate object center
        /// </summary>
        public struct CandidateCenter
        {
            /// <summary>
            /// Candidate center location
            /// </summary>
            public Vector3 Center;

            /// <summary>
            /// Confidence score for center estimate
            /// </summary>
            public double Confidence;

            /// <summary>
            /// Initializes a new instance of the <see cref="CandidateCenter"/> struct.
            /// </summary>
            /// <param name="x">Element x position</param>
            /// <param name="y">Element y position</param>
            /// <param name="z">Element z position</param>
            /// <param name="confidence">Element confidence</param>
            public CandidateCenter(double x, double y, double z, double confidence)
            {
                this.Center = new Vector3(x, y, z);
                this.Confidence = confidence;
            }
        }
    }
}
